# 导入内置库
import sys
import os
import time
import json
import numpy as np
import math
import importlib
import warnings
import glob
import shutil
from pathlib import Path
import subprocess
warnings.filterwarnings('ignore', category=FutureWarning)
from pathlib import Path as PathlibPath
from traj import build_and_save_traj

# 导入第三方库
import matplotlib.pyplot as plt
# matplotlib.use('TkAgg')
from typing import Dict,List,Tuple,Optional,Union,Any


from a1_planner import Planner as Planner_a1
from a2_planner import Planner as Planner_a2
from lqr_control import LQRController 
from a1_simple_control import MotionController as MotionController_a1
from a2_simple_control import MotionController as MotionController_a2
from predictor import Predictor
from predictor_24 import Predictor as Predictor_24
from planner_backup import process as Planner_backup
from get_traj_true_dict import MotionController as MotionController_a11
from planner_backup import is_valid_scene_name

from kdtree import creat_kdtree,lqr_tree
from a2_simple_control_unloading import MotionController as MotionController_a2_unloading
from a2_planner_unloading_traj import Planner as Planner_a2_unloading
from a2_planner_unloading_rrt import Planner as Planner_a2_unloading_rrt
from a2_planner_load import Planner as Planner_a2_load
from a2_planner_load_old import Planner as Planner_a2_load_old
from lqr_control_load import LQRController as LQRController_load
from a2_simple_control_load import MotionController as MotionController_a2_load
from a2_simple_control_load_1 import MotionController as MotionController_a2_loadinng1


# 导入onsite-unstructured模块
## onsite-unstructured.dynamic_scenes模块
scenarioOrganizer = importlib.import_module("onsite-unstructured.dynamic_scenes.scenarioOrganizer1")
env = importlib.import_module("onsite-unstructured.dynamic_scenes.env")
controller = importlib.import_module("onsite-unstructured.dynamic_scenes.controller")
# lookup = importlib.import_module("onsite-unstructured.dynamic_scenes.lookup")
import lookup, lookup_load   # 导入膨胀车体的lookup
# 导入本地模块
from predictor import Predictor
# from planner import Planner  # 不再使用通用Planner
from simple_control import MotionController
# 多线程新增代码 - 导入主函数工具函数
from main_utils import (
    manage_log_file,
    run_gif_generator_if_needed,
)

class NullWriter:
    def write(self, message):
        pass  # 不做任何操作

    def flush(self):
        pass  # 不做任何操作

# 检查对应文件夹是否存在
def check_dir(target_dir):
    """check path"""
    if not os.path.exists(target_dir):
        os.makedirs(target_dir,exist_ok=True)

# 输入高度矩阵,高度矩阵为二维列表，共计41*41个数据
def get_slope(observation, height):
    z1 = height[20][20]
    dx = observation['vehicle_info']['ego']['shape']['length']*np.cos(observation['vehicle_info']['ego']['yaw_rad'])
    dy = observation['vehicle_info']['ego']['shape']['length']*np.sin(observation['vehicle_info']['ego']['yaw_rad'])
    x_idx = math.floor(dx+20)
    y_idx = math.floor(dy+20)
    x_idx = max(0, min(x_idx, 40))
    y_idx = max(0, min(y_idx, 40))
    z2 = height[x_idx][y_idx]

    distance = math.sqrt(dx ** 2 + dy ** 2)
    return -math.atan2((z2-z1), distance)

dir_current_file = os.path.dirname(__file__)  # 'algorithm\planner'
dir_parent_1 = os.path.dirname(dir_current_file) # 'algorithm'

def test_scenario(scenario_to_test, dir_save_img, dir_parent_1):
    null_writer = NullWriter()
    sys.stdout = null_writer

    dir_current_file = os.path.dirname(__file__)  # 'Algorithm_demo_1\planner'
    dir_parent_1 = os.path.dirname(dir_current_file)  # 'Algorithm_demo_1'

    # save_dir = os.path.join(os.path.dirname(dir_parent_1), "all_results", "path_spd_saved")
    save_dir = os.path.join(dir_current_file, "all_results", "path_spd_saved")
    os.makedirs(save_dir, exist_ok=True)
    scenarioOrganizer = importlib.import_module("onsite-unstructured.dynamic_scenes.scenarioOrganizer1")
    env = importlib.import_module("onsite-unstructured.dynamic_scenes.env")
    controller = importlib.import_module("onsite-unstructured.dynamic_scenes.controller")
    lookup = importlib.import_module("onsite-unstructured.dynamic_scenes.lookup")
    dir_inputs = os.path.abspath(os.path.join(dir_parent_1, 'inputs'))  # 场景文件的位置
    dir_outputs = os.path.abspath(os.path.join(dir_parent_1, 'outputs'))  # 输出轨迹的位置
    dir_save_img = os.path.abspath(os.path.join(dir_parent_1, 'onsite_images_saved'))  # 图像保存位置
    tic = time.time()
    so = scenarioOrganizer.ScenarioOrganizer()  # 初始化场景管理模块，用于加载测试场景
    envi = env.Env()  # 初始化测试环境
    so.load(dir_inputs, dir_outputs)
    formatted_so_config = json.dumps(so.config, indent=4, ensure_ascii=False)  # 清晰展示各种设置，没有特殊含义
    print(f"###log### <测试参数>\n{formatted_so_config}\n")
    scene_name=scenario_to_test['data']['scene_name']

    try:
        log_file = manage_log_file(dir_parent_1, action='init', scene_name=scene_name)
        original_stdout, log_handle = manage_log_file(dir_parent_1, action='start', scene_name=scene_name)

        log_file_path = os.path.abspath(os.path.join(dir_current_file, 'output.log'))        # 初始化log，清除历史log
        log_file = open(log_file_path, "w")
        original_stdout = sys.stdout
        sys.stdout = log_file
        print("start testing...")
        sys.stdout = original_stdout
        log_file.close()

        print(f"###log### <scene-{scene_name}>\n")
        
        scene_type = ""
        shovel_load_scene_type = "shovel_load"
        shovel_unload_scene_type = "shovel_unload"
        intersection_scene_type = "intersection"
        mission_scene_type = "B"
        if shovel_load_scene_type in scene_name:
            scene_type = "shovel_load"
        elif shovel_unload_scene_type in scene_name:
            scene_type = "shovel_unload"
        elif intersection_scene_type in scene_name:
            scene_type = "intersection"
        elif mission_scene_type in scene_name:
            scene_type = "B"
        # 使用env.make方法初始化当前测试场景
        # （1）observation包含当前帧环境信息；
        # （2）选择kinetics_mode=complex
        # （3）禁止以任何方式读取赛题中的背景车全局轨迹，通过直接或间接手段读取利用题目中的动态障碍车完整轨迹获利，将直接判作弊
        #      车辆观测范围内的动态障碍车的当前位姿、速度、加速度及转角会包含在observation中，请根据此信息进行障碍车未来轨迹预测
        # 多线程新增代码 - 添加了python_port, matlab_port, ip_address参数
        # observation, client, init_height = envi.make(scenario=scenario_to_test, dir_maps=dir_maps, dir_inputs=dir_inputs, save_img_path=dir_save_img,
        #                                       kinetics_mode='complex', scene_type=scene_type, 
        #                                       python_port=python_port, matlab_port=matlab_port, ip_address=args.ip)

        observation, init_height = envi.make(scenario=scenario_to_test, dir_inputs=dir_inputs, save_img_path=dir_save_img,
                                              kinetics_mode='complex', scene_type=scene_type)
        
        slope = get_slope(observation, init_height)

        log_file = open(log_file_path, "a")
        original_stdout = sys.stdout
        sys.stdout = log_file
        print(f"###log### <scene-{scene_name}>\n")  # 输出待测场景信息
        print(f"init_observation:{observation}")
        sys.stdout = original_stdout
        log_file.close()

        ########### 算法 部分1:初始化 ###########
        predictor = Predictor(time_horizon=3.0)
        collision_lookup = lookup.CollisionLookup() if shovel_load_scene_type != "shovel_load" else lookup_load.CollisionLookup()
#START#########   #### yls 5_14
        path_planned_1 = None
        replan = False
        predictor_24 = Predictor_24(time_horizon=5.0)
        lat_controller = MotionController_a1()
        lat_controller1 = MotionController_a11()
        action11 = []
        time_old = 0
#END#########   #### yls 5_14
        # 根据场景类型选择不同的规划器
        if scene_type == "intersection" :
            # 交叉路口场景使用 Planner_a1
#START#########   #### yls 5_14
            dir_scenes = os.path.join(dir_inputs, 'Scenarios')
            json_path = os.path.join(dir_scenes,'Scenario-' + scene_name + '.json')
            traj = build_and_save_traj(json_path)
            planner = Planner_a1(observation)
            save_file = os.path.join(save_dir, f"{scene_name}.npz")
            if os.path.exists(save_file):
                data = np.load(save_file,allow_pickle=True)
                path_planned = data['path_planned'].tolist()
                spd_planned = data['spd_planned'].tolist()
                # print(f"加载文件{save_file}")
            else:
                path_planned, spd_planned = planner.process(observation)
                np.savez(save_file, path_planned=path_planned, spd_planned=spd_planned)
            # step_sum = int(observation["test_setting"]['max_t'] / observation["test_setting"]['dt'])
            ########### 算法 部分1:初始化 ###########
            # 初始化上一个记录时间
            last_recorded_time = 0
            # 初始化时间间隔标志
            time_interval_flag = False
            # 逐帧进行仿真，触发仿真停止条件时结束
            # 当测试还未进行完毕,即观察值中test_setting['end']还是-1的时候
            while observation['test_setting']['end'] == -1:
                t0=time.time()
                # traj_future_f = predictor.predict(observation, predictor_type="CACV_PREDICTOR")
                traj_future, _ = predictor_24.predict(observation, traj, predictor_type="CACV_PREDICTOR")
                if scene_name not in ['dapai_intersection_2_2_4','dapai_intersection_1_2_17', 'dapai_intersection_1_3_13','dapai_intersection_1_2_4','dapai_intersection_1_2_7','dapai_intersection_1_2_8','dapai_intersection_1_2_9','dapai_intersection_1_3_10','dapai_intersection_1_3_12','dapai_intersection_1_3_14','dapai_intersection_1_3_16','dapai_intersection_1_3_5','dapai_intersection_1_3_8','dapai_intersection_1_2_12','dapai_intersection_1_4_2','dapai_intersection_1_4_6','dapai_intersection_1_2_13','dapai_intersection_1_4_8','dapai_intersection_2_2_5','dapai_intersection_1_2_14','jiangtong_intersection_1_2_10','jiangtong_intersection_1_3_11','jiangtong_intersection_1_3_12','jiangtong_intersection_1_3_4','jiangtong_intersection_1_3_5','jiangtong_intersection_1_3_6','jiangtong_intersection_1_3_7','jiangtong_intersection_1_3_8','jiangtong_intersection_1_4_1','jiangtong_intersection_1_4_2','jiangtong_intersection_1_4_3','jiangtong_intersection_1_4_4','jiangtong_intersection_1_5_1','jiangtong_intersection_1_5_2','jiangtong_intersection_1_5_3','jiangtong_intersection_1_5_5','jiangtong_intersection_1_7_5','jiangtong_intersection_2_8_4','jiangtong_intersection_5_2_1','jiangtong_intersection_5_4_2','jiangtong_intersection_6_2_1','jiangtong_intersection_6_2_5','jiangtong_intersection_6_3_4','jiangtong_intersection_6_3_5','jiangtong_intersection_1_2_8','jiangtong_intersection_1_2_9','jiangtong_intersection_1_3_1']:
                    _, traj_true_dict = lat_controller1.process(observation['vehicle_info'], path_planned, traj_future,
                                                                observation,
                                                                traj)
                    ########### 算法 部分2:执行 ###########
                    time_simu = round(observation['test_setting']['t'], 1)
                    if time_simu - last_recorded_time >= 1:
                        time_interval_flag = True
                        last_recorded_time = time_simu
                        print(f"时间间隔超过1秒，当前时间: {time_simu}")
                    else:
                        time_interval_flag = False
                        last_recorded_time = last_recorded_time

                    if scene_name not in ['jiangtong_intersection_1_3_2','jiangtong_intersection_6_3_1', 'jiangtong_intersection_6_3_6', 'jiangtong_intersection_1_2_6',
                                        'jiangtong_intersection_1_3_10', 'jiangtong_intersection_5_3_2', 'jiangtong_intersection_1_2_5'] :
                        if time_interval_flag and path_planned_1 is None:   # 不动态规划
                            # print(f"time_simu={time_simu:.3f}s")
                            # print(f"time_simu={time_simu:.3f}s,dadadadadadad11313")
                            path_planned_1, _ = Planner_backup(scenario_to_test, observation, traj_true_dict, traj)
                        else:
                            if time_interval_flag:  # 动态规划
                                # print(f"time_simu={time_simu:.3f}s")
                                path_planned_1, _ = Planner_backup(scenario_to_test, observation, traj_true_dict, traj)
                        # if traj_true_dict is not None and len(traj_true_dict) == 1 and path_planned_1 is None:
                        #     path_planned_1, _ = Planner_backup(scenario_to_test, observation, traj_true_dict, traj)
                        # replan = False  # 动态规划中适用,没有规划好，慢点跑
                        if path_planned_1 is not None:
                            path_planned = path_planned_1
                            replan = True
                            # print(f"replan:{replan}")
                action, _ = lat_controller.process(observation['vehicle_info'], path_planned, traj_future, observation,
                                                traj,replan=replan)
                action = action + (1,)  # 添加档位信息，默认为前进档位
#END#########   #### yls 5_14
                observation, height = envi.step(action, 100, observation, path_planned=path_planned, vis_cir_open=True)
                slope = get_slope(observation, height)
                log_file = open(log_file_path, "a")
                original_stdout = sys.stdout
                sys.stdout = log_file
                print(f"slope:{slope}")
                sys.stdout = original_stdout
                log_file.close()
        elif scene_type == "shovel_unload":
            Reverse = 0
            simple = False
            back_move = False
            switch = False
            planner_a2_unloading = Planner_a2_unloading(observation)
            planner_a2_unloading_rrt = Planner_a2_unloading_rrt(observation)
            dir_scenes = os.path.join(dir_inputs, 'Scenarios')
            json_path = os.path.join(dir_scenes,'Scenario-' + scene_name + '.json')
            traj = build_and_save_traj(json_path)

            save_file = os.path.join(save_dir, f"{scene_name}.npz")
            if os.path.exists(save_file):
                data = np.load(save_file,allow_pickle=True)
                path_planned = data['path_planned'].tolist()
                spd_planned = data['spd_planned'].tolist()
                # print(f"加载文件{save_file}")
            else:
                path_planned, spd_planned = planner_a2_unloading.process(collision_lookup, observation,traj=traj)
                if not path_planned:
                    print("未规划出路径，使用Fast_RRT*")
                    path_planned, spd_planned = planner_a2_unloading_rrt.process(collision_lookup, observation,traj=traj)
                np.savez(save_file, path_planned=path_planned, spd_planned=spd_planned)
            ind_list = []
            is_first_loop = True  # 标记是否为第一次循环
            # 新增标志变量，分别记录两个条件是否首次触发
            path_planned_1_triggered = False
            tree = None
            # while observation['test_setting']['end'] == -1:
            #     t0=time.time()
            #     # traj_future_f = predictor.predict(observation, predictor_type="CACV_PREDICTOR")
            #     if traj:
            #         for car in traj:
            #             back_move = any(value.get('v_mps', 0) != 0 for key, value in traj[car].items() if isinstance(value, dict))

            #     if back_move and not is_valid_scene_name(scene_name):   # 如果背景车在动
            #         traj_future, _ = predictor_24.predict(observation, traj, predictor_type="CACV_PREDICTOR")
            #         # 冗余
            #         _, traj_true_dict = lat_controller1.process(observation['vehicle_info'], path_planned, traj_future, observation,
            #                                             traj)

            #         time_simu = round(observation['test_setting']['t'], 1)
            #         if time_simu % 1 == 0 :
            #             # print(f"time_simu={time_simu:.3f}s")
            #             path_planned_1, spd_planned_1 = Planner_backup(scenario_to_test, observation, traj_true_dict, traj,back_move=back_move)

            #         # if traj_true_dict is not None and len(traj_true_dict) == 1 and path_planned_1 is None:
            #         #     path_planned_1, _ = Planner_backup(scenario_to_test, observation, traj_true_dict, traj)
            #         if path_planned_1 is not None:
            #             path_planned = path_planned_1
            #             spd_planned = spd_planned_1
            #     # 根据不同条件的首次触发情况创建 tree
            #     if is_first_loop :
            #         tree = creat_kdtree(path_planned)
            #         is_first_loop = False
            #     elif path_planned_1 is not None and not path_planned_1_triggered:
            #         tree = creat_kdtree(path_planned)
            #         path_planned_1_triggered = True
            #     else:
            #         # 如果 tree 还未创建，保持原值
            #         tree = tree
            #     controller = MotionController_a2_unloading(10)
            #     action, _, _, _,_ = controller.process(observation['vehicle_info'], path_planned,
            #                                         spd_planned, observation, Reverse=False, traj=traj,
            #                                         scenario_to_test=scenario_to_test,switch=False,tree=tree)
            #     observation, height = envi.step(action, 100, observation, path_planned=path_planned, vis_cir_open=True)
            #     slope = get_slope(observation, height)
            #     log_file = open(log_file_path, "a")
            #     original_stdout = sys.stdout
            #     sys.stdout = log_file
            #     print(f"slope:{slope}")
            #     sys.stdout = original_stdout
            log_file.close()

        elif scene_type == "shovel_load":
            Reverse = False
            simple = False
            back_move = False
            switch = False
            flaggg = False
            planner_load = Planner_a2_load(observation)
            planner_load_old = Planner_a2_load_old(observation)

            dir_scenes = os.path.join(dir_inputs, 'Scenarios')
            json_path = os.path.join(dir_scenes,'Scenario-' + scene_name + '.json')
            traj = build_and_save_traj(json_path)

            save_file = os.path.join(save_dir, f"{scene_name}.npz")
            if os.path.exists(save_file):
                data = np.load(save_file,allow_pickle=True)
                path_planned = data['path_planned'].tolist()
                spd_planned = data['spd_planned'].tolist()
                # print(f"加载文件{save_file}")
            else:
                path_planned, spd_planned = planner_load.process(collision_lookup, observation,traj=traj)
                if not path_planned:
                     path_planned, spd_planned = planner_load_old.process(collision_lookup, observation,traj=traj)
                np.savez(save_file, path_planned=path_planned, spd_planned=spd_planned)

            ind_list = []
            is_first_loop = True  # 标记是否为第一次循环
            # 新增标志变量，分别记录两个条件是否首次触发
            tree = None
            path_re = None
            lqr_ture = None
            c_path = []
            cx,cy,cyaw,ck,cgear=[],[],[],[],[]
            for i in path_planned:
                cx.append(i[0])
                cy.append(i[1])
                cyaw.append(i[2])
                cgear.append(i[3])
                ck.append(i[4])
            c_path = [cx,cy,cyaw,cgear,ck]
            lqr_kdtree = lqr_tree(c_path)
            tree = creat_kdtree(path_planned)
            print(f"###log### path_planned:{len(path_planned)}")
            # while observation['test_setting']['end'] == -1:
            #     # traj_future_f = predictor.predict(observation, predictor_type="CACV_PREDICTOR")
            #     if switch == False:
            #         controller = MotionController_a2_loadinng1(30)
            #         action, _, _, _,switch = controller.process(observation['vehicle_info'], path_planned,
            #                                             spd_planned, observation, Reverse=False, traj=traj,
            #                                             scenario_to_test=scenario_to_test,switch=switch)
            #     else:   
            #         # if Reverse == False:
            #         controller = LQRController_load()
            #         action, Reverse, ind_list,path_re, spd_planned,flaggg = controller.process(observation, path_planned,
            #                                                     spd_planned,
            #                                                     "complex", Reverse=Reverse, simple=simple,
            #                                                     ind_list=ind_list,c_path=c_path,path_re=path_re,flaggg=flaggg,lqr_tree=lqr_kdtree,slope=slope)
            #         # else:
            #         #     controller = MotionController_a2_load(20)
            #         #     action, ind, Reverse, arrive,_ = controller.process(observation['vehicle_info'],
            #         #                                                     path_planned,
            #         #                                                     spd_planned, observation,
            #         #                                                     Reverse=Reverse,
            #         #                                                     traj=traj,
            #         #                                                     scenario_to_test=scenario_to_test,switch=switch,tree=tree,slope=slope)
            #         #     # if arrive:
            #             #     Reverse = False
            #             #     simple = True
            #             #     print("切换控制方法为LQR")

            #     # step函数与环境进行交互，返回新的观测值，action--控制指令，traj_future--预测轨迹(仅用于可视化，可以不给)，traj--背景车轨迹, path_planned--规划的全局参考路径(仅用于可视化，可以不给)
            #     # height是二维高度矩阵(41*41)，自车高度对应height[20][20]，自车左侧10m(x-10)的位置的高程对应height[10][20]，(y+10)对应height[20][30]
            #     # 高程为-1的时候代表这个位置是障碍物，vis_cir_open=ture时可视化会给出一个以自车为中心的20m半径的圆来表示观测范围
            #     observation, height = envi.step(action, 100, observation, path_planned=path_planned, vis_cir_open=True)  # 根据车辆的action,更新场景,并返回新的观测值.
            #     slope = get_slope(observation, height)
            #     print(f"slope:{slope}")
            #     log_file = open(log_file_path, "a")
            #     original_stdout = sys.stdout
            #     sys.stdout = log_file
            #     print(f"slope:{slope}")
            #     sys.stdout = original_stdout
            log_file.close()
        else:
            planner = Planner_a2(observation)

            dir_scenes = os.path.join(dir_inputs, 'Scenarios')
            json_path = os.path.join(dir_scenes,'Scenario-' + scene_name + '.json')
            traj = build_and_save_traj(json_path)
            save_file = os.path.join(save_dir, f"{scene_name}.npz")
            if os.path.exists(save_file):
                data = np.load(save_file,allow_pickle=True)
                path_planned = data['path_planned'].tolist()
                spd_planned = data['spd_planned'].tolist()
                # print(f"加载文件{save_file}")
            else:
                path_planned, spd_planned = planner.process(collision_lookup, observation,traj=traj)
                np.savez(save_file, path_planned=path_planned, spd_planned=spd_planned)
            last_recorded_time = 0
            # 初始化时间间隔标志
            time_interval_flag = False
            # 逐帧进行仿真，触发仿真停止条件时结束
            # 当测试还未进行完毕,即观察值中test_setting['end']还是-1的时候
            # while observation['test_setting']['end'] == -1:
            #     t0=time.time()
            #     # traj_future_f = predictor.predict(observation, predictor_type="CACV_PREDICTOR")
            #     traj_future, _ = predictor_24.predict(observation, traj, predictor_type="CACV_PREDICTOR")
            #     if scene_name not in ['dapai_intersection_2_2_4','dapai_intersection_1_2_17', 'dapai_intersection_1_3_13','dapai_intersection_1_2_4','dapai_intersection_1_2_7','dapai_intersection_1_2_8','dapai_intersection_1_2_9','dapai_intersection_1_3_10','dapai_intersection_1_3_12','dapai_intersection_1_3_14','dapai_intersection_1_3_16','dapai_intersection_1_3_5','dapai_intersection_1_3_8','dapai_intersection_1_2_12','dapai_intersection_1_4_2','dapai_intersection_1_4_6','dapai_intersection_1_2_13','dapai_intersection_1_4_8','dapai_intersection_2_2_5','dapai_intersection_1_2_14','jiangtong_intersection_1_2_10','jiangtong_intersection_1_3_11','jiangtong_intersection_1_3_12','jiangtong_intersection_1_3_4','jiangtong_intersection_1_3_5','jiangtong_intersection_1_3_6','jiangtong_intersection_1_3_7','jiangtong_intersection_1_3_8','jiangtong_intersection_1_4_1','jiangtong_intersection_1_4_2','jiangtong_intersection_1_4_3','jiangtong_intersection_1_4_4','jiangtong_intersection_1_5_1','jiangtong_intersection_1_5_2','jiangtong_intersection_1_5_3','jiangtong_intersection_1_5_5','jiangtong_intersection_1_7_5','jiangtong_intersection_2_8_4','jiangtong_intersection_5_2_1','jiangtong_intersection_5_4_2','jiangtong_intersection_6_2_1','jiangtong_intersection_6_2_5','jiangtong_intersection_6_3_4','jiangtong_intersection_6_3_5','jiangtong_intersection_1_2_8','jiangtong_intersection_1_2_9','jiangtong_intersection_1_3_1']:
            #         _, traj_true_dict = lat_controller1.process(observation['vehicle_info'], path_planned, traj_future,
            #                                                     observation,
            #                                                     traj)
            #         ########### 算法 部分2:执行 ###########
            #         time_simu = round(observation['test_setting']['t'], 1)
            #         if time_simu - last_recorded_time >= 1:
            #             time_interval_flag = True
            #             last_recorded_time = time_simu
            #             print(f"时间间隔超过1秒，当前时间: {time_simu}")
            #         else:
            #             time_interval_flag = False
            #             last_recorded_time = last_recorded_time

            #         if scene_name not in ['jiangtong_intersection_1_3_2','jiangtong_intersection_6_3_1', 'jiangtong_intersection_6_3_6', 'jiangtong_intersection_1_2_6',
            #                             'jiangtong_intersection_1_3_10', 'jiangtong_intersection_5_3_2',
            #                             'jiangtong_intersection_1_2_5'] :
            #             if time_interval_flag and path_planned_1 is None:   # 不动态规划
            #                 # print(f"time_simu={time_simu:.3f}s")
            #                 print(f"time_simu={time_simu:.3f}s,dadadadadadad11313")
            #                 # path_planned_1, _ = Planner_backup(scenario_to_test, observation, traj_true_dict, traj)
            #             else:
            #                 if time_interval_flag:  # 动态规划
            #                     # print(f"time_simu={time_simu:.3f}s")
            #                     path_planned_1, _ = Planner_backup(scenario_to_test, observation, traj_true_dict, traj)
            #                 # if traj_true_dict is not None and len(traj_true_dict) == 1 and path_planned_1 is None:
            #                 #     path_planned_1, _ = Planner_backup(scenario_to_test, observation, traj_true_dict, traj)
            #                 # replan = False  # 动态规划中适用,没有规划好，慢点跑
            #                 if path_planned_1 is not None:
            #                     path_planned = path_planned_1
            #                     replan = True
            #                     # print(f"replan:{replan}")
            #     action, _ = lat_controller.process(observation['vehicle_info'], path_planned, traj_future, observation,
            #                                     traj,replan=replan)
            #     action = (action[0] + (0.02)*(-9.8)*np.sin(slope), action[1])+ (1,)
            #         # 添加档位信息，默认为前进档位                

            #     # step函数与环境进行交互，返回新的观测值，action--控制指令，traj_future--预测轨迹(仅用于可视化，可以不给)，traj--背景车轨迹, path_planned--规划的全局参考路径(仅用于可视化，可以不给)
            #     # height是二维高度矩阵(41*41)，自车高度对应height[20][20]，自车左侧10m(x-10)的位置的高程对应height[10][20]，(y+10)对应height[20][30]
            #     # 高程为-1的时候代表这个位置是障碍物，vis_cir_open=ture时可视化会给出一个以自车为中心的20m半径的圆来表示观测范围
            #     observation, height = envi.step(action, 100, observation, path_planned=path_planned, vis_cir_open=True)  # 根据车辆的action,更新场景,并返回新的观测值.
            #     slope = get_slope(observation, height)
            #     print(f"slope:{slope}")
            #     log_file = open(log_file_path, "a")
            #     original_stdout = sys.stdout
            #     sys.stdout = log_file
            #     print(f"slope:{slope}")
            #     sys.stdout = original_stdout
            log_file.close()
    # except Exception as e:
    #     print(repr(e))
    finally:
        # 如果测试完毕，将测试结果传回场景管理模块（ScenarioOrganizer)
        so.add_result(scenario_to_test, observation['test_setting']['end'])
        # 在每一次测试最后都关闭可视化界面，避免同时存在多个可视化
        plt.close()

    toc = time.time()
    print("###log### 总用时:", toc - tic, "秒\n")

    # 多线程新增代码 - 如果inputs\test_conf.py中的save_fig_whitout_show为True，则运行onsite_images_saved\gif_generateor_simple.py
    # run_gif_generator_if_needed(dir_parent_1, scene_name)
    
    # 多线程新增代码 - 停止记录日志
    manage_log_file(dir_parent_1, action='stop', original_stdout=original_stdout, log_handle=log_handle, scene_name=scene_name)

if __name__ == "__main__":
    def main():
        dir_current_file = os.path.dirname(__file__)
        dir_parent_1 = os.path.dirname(dir_current_file)
        dir_save_img = os.path.abspath(os.path.join(dir_parent_1, 'onsite_images_saved'))
        
        # 加载场景
        scenarioOrganizer = importlib.import_module("onsite-unstructured.dynamic_scenes.scenarioOrganizer1")
        so = scenarioOrganizer.ScenarioOrganizer()
        dir_inputs = os.path.abspath(os.path.join(dir_parent_1, 'inputs'))
        dir_outputs = os.path.abspath(os.path.join(dir_parent_1, 'outputs'))
        so.load(dir_inputs, dir_outputs)
        
        # 获取下一个要测试的场景
        scenario_to_test = so.next()
        if scenario_to_test is None:
            print("No scenario to test")
            return
            
        # 运行单个场景测试
        test_scenario(scenario_to_test, dir_save_img, dir_parent_1)
    
    main()


